#include "main.h"
#include <string.h>
#include "gpio.h"
#include "usart.h"
#include "tim.h"
#include "ufcs.h"
#include "ufcs_driver.h"
#include "led.h"

static uint8_t MSG_ID = 0;

static volatile uint8_t OutputCap_Number = 0;
static uint8_t OutputCap_Buff[8][8] = { 0 };

#define CRC_8_POLYNOMIAL 0x29 //X8 + X5 + X3 + 1
unsigned char UFCS_CRC8_Calculate(unsigned char *pData, unsigned char Size)
{
	unsigned char i;
	unsigned char rCRC = 0;
	while(Size--)
	{
		rCRC ^= *pData++;
		for (i = 8; i > 0; --i)
		{
			if (rCRC & 0x80) rCRC = (rCRC << 1) ^ CRC_8_POLYNOMIAL;
			else rCRC = (rCRC << 1);
		}
	}
	return(rCRC);
}

static void UFCS_Send_Data(uint8_t *data, uint8_t len)
{
	static uint8_t ufcs_tx_buff[64] = { 0 };
	ufcs_tx_buff[0] = 0xAA;
	memcpy(ufcs_tx_buff + 1, data, len);
	ufcs_tx_buff[len+1]= UFCS_CRC8_Calculate(data, len);
	HAL_UART_Transmit(&huart2, ufcs_tx_buff, len+2, 0xff);
}

static void UFCS_Send_Control_Message(uint8_t cmd)
{
	uint8_t ufcs_tx_buff[8] = { 0 };
	ufcs_tx_buff[0] = 0xAA;
	ufcs_tx_buff[1] = 0x20 | (MSG_ID << 1);;
	ufcs_tx_buff[2] = 0x08;
	ufcs_tx_buff[3] = cmd;
	ufcs_tx_buff[4]= UFCS_CRC8_Calculate(ufcs_tx_buff + 1, 3);
	
	HAL_UART_Transmit(&huart2, ufcs_tx_buff, 5, 0xff);
}

static void UFCS_Send_Data_Message(uint8_t cmd, uint8_t *data, uint8_t len)
{
	uint8_t ufcs_tx_buff[32] = { 0 };
	ufcs_tx_buff[0] = 0xAA;
	ufcs_tx_buff[1] = 0x20 | (MSG_ID << 1);
	ufcs_tx_buff[2] = 0x09;
	ufcs_tx_buff[3] = cmd;
	ufcs_tx_buff[4] = len;
	memcpy(ufcs_tx_buff + 5, data, len);
	ufcs_tx_buff[5 + len]= UFCS_CRC8_Calculate(ufcs_tx_buff + 1, 4 + len);
	HAL_UART_Transmit(&huart2, ufcs_tx_buff, 6 + len, 0xff);
}

static int UFCS_Recv_Data(uint32_t time_out)
{
	ufcs_recv_idx = 0;
	UFCS_Usart_Recv_Start();
	uint32_t start_tick = HAL_GetTick();
	
	while(ufcs_recv_idx < 5)
	{
		if((HAL_GetTick() - start_tick) > time_out)
		{
			return -1;
		}
	}
	
	int len = 5;
	if((ufcs_recv_buff[2] & 0x01) > 0) // data message
	{
		len += ufcs_recv_buff[4] + 1;
	}
	
	while(ufcs_recv_idx < len)
	{
		if((HAL_GetTick() - start_tick) > time_out)
		{
			return -2;
		}
	}
	
	MSG_ID = (ufcs_recv_buff[1]&0x1E) >> 1;
	return len;
}


int UFCS_Shake_Hand(void)
{
    LED_Set_Protocol(LED_NO);
    
    OutputCap_Number = 0;
	DN_Set_Mode_Out(0);
	DP_Set_Mode_In();
	
	HAL_Delay_ms(100);
	
	DN_Set_Mode_Out(1);  // 2mS
	HAL_Delay_ms(2);
	
	DN_Set_Mode_Out(0);  // 8mS
	HAL_Delay_ms(8);
	
	DN_Set_Mode_Out(1);  // 2mS
	HAL_Delay_ms(2);
	
	DN_Set_Mode_Out(0);  // 8mS
	
	HAL_Delay_ms(4);
	if(DP_Read_Value() == 1)
	{
		return 1;
	}
	
	HAL_Delay_ms(6);
	if(DP_Read_Value() == 0)
	{
		return 2;
	}
	
	DN_Set_Mode_Out(1);
	HAL_Delay_ms(1);
	
	MSG_ID = 0;
	DNDP_Set_Mode_UART();
	
	UFCS_Send_Control_Message(UFCS_CONTROL_CMD_PING); // ping
	if(UFCS_Recv_Data(UFCS_tACKReceive) < 0 || ufcs_recv_buff[3] != UFCS_CONTROL_CMD_ACK) return 3;
	HAL_Delay(2);
	const uint8_t WTD[2] = {0x00, 0x00};
	UFCS_Send_Data_Message(UFCS_DATA_CMD_CONFIG_WTD, (uint8_t*)WTD, 2); // Config Watch Dog
	if(UFCS_Recv_Data(UFCS_tACKReceive) < 0 || ufcs_recv_buff[3] != UFCS_CONTROL_CMD_ACK) return 4; // Wait ACK
	if(UFCS_Recv_Data(UFCS_tReceiverResponse) < 0 || ufcs_recv_buff[3] != UFCS_CONTROL_CMD_ACCEPT) return 5; // Wait Accept
	HAL_Delay_us(800);
	UFCS_Send_Control_Message(UFCS_CONTROL_CMD_ACK);
    LED_Set_Protocol(LED_UFCS);
	return 0;
}

int UFCS_Get_Output_Cap(uint8_t* data)
{
	int len = 0;
	UFCS_Send_Control_Message(UFCS_CONTROL_CMD_GET_OUTPUT_CAP); // Get Output Cap
	if(UFCS_Recv_Data(UFCS_tACKReceive) < 0 || ufcs_recv_buff[3] != UFCS_CONTROL_CMD_ACK) return 1;
	
	if(UFCS_Recv_Data(UFCS_tReceiverResponse) < 0 || ufcs_recv_buff[3] != UFCS_DATA_CMD_OUTPUT_CAP) return 2;
	
	len = ufcs_recv_buff[4];
	OutputCap_Number = len / 8;
    
	memcpy(OutputCap_Buff, ufcs_recv_buff + 5, len);
    
	if(data != NULL) memcpy(data, ufcs_recv_buff + 5, len);
	
	HAL_Delay_us(800);
	
	UFCS_Send_Control_Message(UFCS_CONTROL_CMD_ACK);
	
	return len;
}

int UFCS_Request(uint8_t num, uint16_t Volt, uint16_t Curr)
{
	uint8_t tmp[8] = { 0 };
	Volt = Volt / 10;
	Curr = Curr / 10;
	
	tmp[0] = num << 4;
	
	tmp[4] = Volt >> 8;
	tmp[5] = Volt & 0xFF;
	
	tmp[6] = Curr >> 8;
	tmp[7] = Curr & 0xFF;
	
	UFCS_Send_Data_Message(UFCS_DATA_CMD_REQUEST, (uint8_t*)tmp, 8);
	if(UFCS_Recv_Data(UFCS_tACKReceive) < 0 || ufcs_recv_buff[3] != UFCS_CONTROL_CMD_ACK) return 1; // Wait ACK
	
	if(UFCS_Recv_Data(UFCS_tReceiverResponse) < 0 )  return 2; //Recv Error
	UFCS_Send_Control_Message(UFCS_CONTROL_CMD_ACK); // Response ACK
	if(ufcs_recv_buff[3] != UFCS_CONTROL_CMD_ACCEPT) return 3; // Wait Accept
	
	if(UFCS_Recv_Data(UFCS_tPowerSupply) < 0 )  return 4; //Recv Error
	UFCS_Send_Control_Message(UFCS_CONTROL_CMD_ACK); // Response ACK
	if(ufcs_recv_buff[3] != UFCS_CONTROL_CMD_POWER_READY) return 5; // Wait Power Ready
	return 0;
}

int Get_Source_Info(uint8_t *data)
{
	UFCS_Send_Control_Message(UFCS_CONTROL_CMD_GET_SOURCE_INFO);
	if(UFCS_Recv_Data(UFCS_tACKReceive) < 0 )  return 1; //Recv Error
	if(ufcs_recv_buff[3] != UFCS_CONTROL_CMD_ACK) return 2; // Wait ACK
	
	if(UFCS_Recv_Data(UFCS_tReceiverResponse) < 0 )  return 3; //Recv Error
	UFCS_Send_Control_Message(UFCS_CONTROL_CMD_ACK); // Response ACK
	if(ufcs_recv_buff[3] != UFCS_DATA_CMD_SOURCE_INFO) return 4; // Wait ACK
	
	memcpy(data, ufcs_recv_buff + 5, 8);
	
	return 8;
}

int UFCS_Exit(void)
{
	UFCS_Send_Control_Message(UFCS_CONTROL_CMD_EXIT_UFCS_MODE);
	if(UFCS_Recv_Data(UFCS_tACKReceive) < 0 )  return 1; //Recv Error
	if(ufcs_recv_buff[3] != UFCS_CONTROL_CMD_ACK) return 2; // Wait ACK
	return 0;
}

int UFCS_Set_Vol(uint16_t volt) // mV
{
    
    uint16_t max_volt, min_volt, max_curr;
	if(OutputCap_Number < 1) return -1;
	for(int i = 0; i < OutputCap_Number; i ++)
	{
        max_volt = (OutputCap_Buff[i][1]*256 + OutputCap_Buff[i][2]) * 10;
        min_volt = (OutputCap_Buff[i][3]*256 + OutputCap_Buff[i][4]) * 10;
		if( min_volt <= volt && volt <= max_volt) // match
        {
            max_curr = (OutputCap_Buff[i][5]*256 + OutputCap_Buff[i][6]) * 10;
            return UFCS_Request( i + 1, volt, max_curr);
        }
	}
    return -1;
}
